
#include "MultibodySystem.h"


using namespace std;
using namespace Eigen;

int main()
{
	char instr[200];
	cout << "Please Enter Input File Name(.json):" << endl;
	cin >> instr;
	string fname(instr);
	string outdatafile = fname;
	outdatafile.erase(outdatafile.find_last_of('.'), 5);
	clock_t start = clock();
	MBFileParser mbparser;
	try
	{
		mbparser.Read(fname);
		mbparser.Simulate();
		cout << "The computation costs " << (double)(clock() - start) / CLOCKS_PER_SEC << " s." << endl;
		mbparser.SaveDataAs(outdatafile, false);
		cout << "The Output Data File is " << outdatafile << ".txt" << endl;
	}
	catch (const MBException& mbexp)
	{
		cout << mbexp.what() << endl;
		cout << "Simulation Failed." << endl;
	}
	catch (const std::exception& stdexp)
	{
		cout << stdexp.what() << endl;
		cout << "Simulation Failed." << endl;
	}
	system("pause");
	///*create body*/
	//double m1, m2;
	//m1 = m2 = 54;
	//Matrix3d J1, J2;
	//J1.setZero();
	//J1.diagonal() = Vector3d(18, 18, 1);
	//J2.setZero();
	//J2.diagonal() = Vector3d(1, 18, 18);
	//Body* pb1 = new RigidBody(m1,J1);
	//Body* pb2 = new RigidBody(m2, J2);
	//Body* pb = new BaseBody();
	//pb1->SetID(1);
	//pb2->SetID(2);
	///*create joint*/
	//Matrix3d C1P, C0Q, C1Q, C2P;
	//C0Q.setZero();
	//C0Q(0, 1) = C0Q(1, 2) = C0Q(2, 0) = 1;
	//C1P = C0Q;
	//C1Q.setZero();
	//C1Q(0, 1) = -1;
	//C1Q(1, 0) = C1Q(2, 2) = 1;
	//C2P = C1Q;
	//Vector3d rho0Q, rho1P, rho1Q, rho2P;
	//rho0Q.setZero();
	//rho1P.setZero();
	//rho1P(2) = -1;
	//rho1Q.setZero();
	//rho1Q(2) = 1;
	//rho2P.setZero();
	//rho2P(0) = -1;
	//JointBase* H1 = new Revolute(pb1, pb, rho1P, rho0Q);
	//JointBase* H2 = new Revolute(pb2, pb1, rho2P, rho1Q);
	//H1->setCjQ(C0Q);
	//H1->setCiP(C1P);
	//H2->setCjQ(C1Q);
	//H2->setCiP(C2P);
	///*create force*/
	//ForceBase* fg1 = new Gravity(pb1);
	//ForceBase* fg2 = new Gravity(pb2);
	//VectorXd M1(1), M2(1);
	//M1 << 100;
	//M2 << 500;
	//JointForce* jf1 = new JointForce(H1, M1);
	//JointForce* jf2 = new JointForce(H2, M2);
	///*create MBSystem and calculate*/
	//MBSystem mbs;
	//mbs.Add(H1);
	//mbs.Add(H2);
	//mbs.Add(fg1);
	//mbs.Add(fg2);
	//mbs.Add(jf1);
	//mbs.Add(jf2);
	//VectorXd y0(2), dy0(2);
	//y0.setZero();
	//dy0.setZero();
	//mbs.Sety0(y0);
	//mbs.Setdy0(dy0);
	//mbs.SetTimeInterval(0, 4, 4000);
	//mbs.Calculate();
	//mbs.SaveAs("twopole");
	///*free memory*/
	//delete pb1, pb2, pb;
	//delete H1, H2;
	//delete fg1, fg2;
	//delete jf1, jf2;
	return 0;
}